/**
 * @file bsp_usb.c
 * @brief USB CDC driver implementation for H743-V2
 * @author Flight Control Team
 * @date 2025-10-31
 */

#include "bsp_usb.h"
#include "usbd_core.h"
#include "usbd_desc.h"
#include "usbd_cdc.h"
#include "usbd_cdc_if.h"
#include <string.h>
#include <stdarg.h>
#include <stdio.h>

/* USB Device handle */
USBD_HandleTypeDef hUsbDeviceFS;

/* USB CDC buffer sizes */
#define USB_TX_BUFFER_SIZE  2048
#define USB_RX_BUFFER_SIZE  1024

/* USB state */
static struct {
    USBState_t state;
    uint8_t tx_buffer[USB_TX_BUFFER_SIZE];
    uint8_t rx_buffer[USB_RX_BUFFER_SIZE];
    uint16_t tx_head;
    uint16_t tx_tail;
    uint16_t rx_head;
    uint16_t rx_tail;
    bool initialized;
} usb_ctx = {0};

/**
 * @brief Initialize USB CDC
 */
int BSP_USB_Init(void)
{
    if (usb_ctx.initialized) {
        return 0;
    }

    /* Initialize USB Device Library */
    if (USBD_Init(&hUsbDeviceFS, &FS_Desc, DEVICE_FS) != USBD_OK) {
        return -1;
    }

    /* Register CDC class */
    if (USBD_RegisterClass(&hUsbDeviceFS, &USBD_CDC) != USBD_OK) {
        return -1;
    }

    /* Register CDC interface callbacks */
    if (USBD_CDC_RegisterInterface(&hUsbDeviceFS, &USBD_Interface_fops_FS) != USBD_OK) {
        return -1;
    }

    /* Start USB Device */
    if (USBD_Start(&hUsbDeviceFS) != USBD_OK) {
        return -1;
    }

    /* Initialize buffers */
    memset(usb_ctx.tx_buffer, 0, USB_TX_BUFFER_SIZE);
    memset(usb_ctx.rx_buffer, 0, USB_RX_BUFFER_SIZE);
    usb_ctx.tx_head = 0;
    usb_ctx.tx_tail = 0;
    usb_ctx.rx_head = 0;
    usb_ctx.rx_tail = 0;

    usb_ctx.state = USB_STATE_CONFIGURED;
    usb_ctx.initialized = true;

    return 0;
}

/**
 * @brief Deinitialize USB CDC
 */
void BSP_USB_DeInit(void)
{
    if (!usb_ctx.initialized) {
        return;
    }

    /* Stop and deinitialize USB Device */
    USBD_Stop(&hUsbDeviceFS);
    USBD_DeInit(&hUsbDeviceFS);

    usb_ctx.initialized = false;
    usb_ctx.state = USB_STATE_DISCONNECTED;
}

/**
 * @brief Get USB connection state
 */
USBState_t BSP_USB_GetState(void)
{
    return usb_ctx.state;
}

/**
 * @brief Check if USB is connected
 */
bool BSP_USB_IsConnected(void)
{
    return (usb_ctx.state == USB_STATE_CONNECTED ||
            usb_ctx.state == USB_STATE_CONFIGURED);
}

/**
 * @brief Send data via USB CDC
 */
int BSP_USB_Send(const uint8_t *data, uint16_t len)
{
    if (!usb_ctx.initialized || data == NULL || len == 0) {
        return -1;
    }
    
    if (!BSP_USB_IsConnected()) {
        return -1;
    }

    /* Use CDC_Transmit_FS function from usbd_cdc_if.c */
    extern uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len);

    uint8_t result = CDC_Transmit_FS((uint8_t*)data, len);
    if (result == USBD_OK) {
        return len;
    } else if (result == USBD_BUSY) {
        return 0;  // Busy, try again later
    } else {
        return -1;  // Error
    }
}

/**
 * @brief Send string via USB CDC
 */
int BSP_USB_SendString(const char *str)
{
    if (str == NULL) {
        return -1;
    }
    
    return BSP_USB_Send((const uint8_t *)str, strlen(str));
}

/**
 * @brief Receive data from USB CDC
 */
int BSP_USB_Receive(uint8_t *data, uint16_t len)
{
    if (!usb_ctx.initialized || data == NULL || len == 0) {
        return -1;
    }
    
    uint16_t received = 0;
    
    /* Read data from receive buffer */
    while (received < len && usb_ctx.rx_tail != usb_ctx.rx_head) {
        data[received++] = usb_ctx.rx_buffer[usb_ctx.rx_tail];
        usb_ctx.rx_tail = (usb_ctx.rx_tail + 1) % USB_RX_BUFFER_SIZE;
    }
    
    return received;
}

/**
 * @brief Get number of bytes available in receive buffer
 */
uint16_t BSP_USB_Available(void)
{
    if (usb_ctx.rx_head >= usb_ctx.rx_tail) {
        return usb_ctx.rx_head - usb_ctx.rx_tail;
    } else {
        return USB_RX_BUFFER_SIZE - usb_ctx.rx_tail + usb_ctx.rx_head;
    }
}

/**
 * @brief Flush transmit buffer
 */
void BSP_USB_Flush(void)
{
    /* Wait for all data to be transmitted */
    while (usb_ctx.tx_head != usb_ctx.tx_tail) {
        // TODO: Wait for USB transmission to complete
    }
}

/**
 * @brief Printf-like function for USB CDC
 */
int BSP_USB_Printf(const char *format, ...)
{
    char buffer[256];
    va_list args;
    
    va_start(args, format);
    int len = vsnprintf(buffer, sizeof(buffer), format, args);
    va_end(args);
    
    if (len > 0) {
        return BSP_USB_Send((uint8_t *)buffer, len);
    }
    
    return -1;
}

/**
 * @brief USB CDC receive callback
 */
void BSP_USB_ReceiveCallback(uint8_t *data, uint16_t len)
{
    if (data == NULL || len == 0) {
        return;
    }
    
    /* Copy received data to buffer */
    for (uint16_t i = 0; i < len; i++) {
        uint16_t next_head = (usb_ctx.rx_head + 1) % USB_RX_BUFFER_SIZE;
        
        /* Check if buffer is full */
        if (next_head == usb_ctx.rx_tail) {
            break;
        }
        
        usb_ctx.rx_buffer[usb_ctx.rx_head] = data[i];
        usb_ctx.rx_head = next_head;
    }
}

/**
 * @brief Redirect printf to USB CDC
 */
int _write(int file, char *ptr, int len)
{
    (void)file;
    
    if (usb_ctx.initialized && BSP_USB_IsConnected()) {
        return BSP_USB_Send((uint8_t *)ptr, len);
    }
    
    return len;
}

